/*
Robotics object oriented package in C++
Copyright (C) 2008-2009  Matrix

This library is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as
published by the Free Software Foundation; either version 2.1 of the
License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston,  MA 02111-1307  USA
*/

#include "StdAfx.h"
#include "Acceleration.h"

namespace RobotPlant { namespace Math{
	Acceleration::Acceleration(void)
	{
	}

	Acceleration::Acceleration(const Acceleration& arg)
	{
		this->LinearAcc = arg.LinearAcc;
		this->AngleAcc = arg.AngleAcc;
	}

	//Acceleration::Acceleration(const ColumnVector& arg)
	//{
	//	if(arg.GetCount() != 6)
	//		throw(new std::exception("Argument arg's count must be 6."));

	//	this->LinearAcc[0] = arg[0];
	//	this->LinearAcc[1] = arg[1];
	//	this->LinearAcc[2] = arg[2];
	//	this->AngleAcc[0] = arg[3];
	//	this->AngleAcc[1] = arg[4];
	//	this->AngleAcc[2] = arg[5];

	//}

	Acceleration::Acceleration(const Vector3& linear,const Vector3& angle)
		:LinearAcc(linear),AngleAcc(angle)
	{

	}

	Acceleration::~Acceleration(void)
	{
	}

	double& Acceleration::operator[](const int index)
	{
		if((index < 0) || (index > 5))
			throw(new std::exception("Argument row index must be between 0 and 5."));

		if (index<3) 
			return LinearAcc[index];
		else
			return AngleAcc[index-3];
	}

	double Acceleration::operator[](const int index) const
	{
		if((index < 0) || (index > 5))
			throw(new std::exception("Argument row index must be between 0 and 5."));

		if (index<3) 
			return LinearAcc[index];
		else
			return AngleAcc[index-3];
	}

	Acceleration& Acceleration::operator = (const Acceleration& arg)
	{
		LinearAcc = arg.LinearAcc;
		AngleAcc = arg.AngleAcc;
		return *this;
	}

	Acceleration& Acceleration::operator += (const Acceleration& arg)
	{
		LinearAcc += arg.LinearAcc;
		AngleAcc += arg.AngleAcc;
		return *this;
	}

	Acceleration& Acceleration::operator -= (const Acceleration& arg)
	{
		LinearAcc -= arg.LinearAcc;
		AngleAcc -= arg.AngleAcc;
		return *this;
	}

	bool operator == (const Acceleration& lhs, const Acceleration& rhs)
	{
		return Equal(lhs,rhs);
	}

	bool operator != (const Acceleration& lhs, const Acceleration& rhs)
	{
		return !Equal(lhs,rhs);
	}

	// addition of Acceleration's
	Acceleration operator + (const Acceleration& lhs, const Acceleration& rhs)
	{
		return Acceleration(lhs.LinearAcc + rhs.LinearAcc, lhs.AngleAcc + rhs.AngleAcc);
	}

	// unary -
	Acceleration operator - (const Acceleration& lhs, const Acceleration& rhs) 
	{
		return Acceleration(lhs.LinearAcc - rhs.LinearAcc, lhs.AngleAcc - rhs.AngleAcc);
	}

	Acceleration operator - (const Acceleration& arg)
	{
		return Acceleration( -arg.LinearAcc, -arg.AngleAcc );
	}

	Acceleration operator * (const Acceleration& lhs, const double rhs)
	{
		return Acceleration(lhs.LinearAcc * rhs, lhs.AngleAcc * rhs);
	}

	Acceleration operator * (const double lhs, const Acceleration& rhs)
	{
		return Acceleration(lhs * rhs.LinearAcc, lhs * rhs.AngleAcc);
	}

	Acceleration operator / (const Acceleration& lhs, const double rhs)
	{
		return Acceleration(lhs.LinearAcc / rhs, lhs.AngleAcc / rhs);
	}


	void Acceleration::SetZero( ) 
	{
		LinearAcc.SetZero();
		AngleAcc.SetZero();
	}

	Acceleration Acceleration::Zero()
	{
		return Acceleration(Vector3::Zero(),Vector3::Zero());
	}

	void Acceleration::ReverseSign()
	{   
		LinearAcc.ReverseSign();
		AngleAcc.ReverseSign();
	}

	Acceleration Acceleration::RefPoint(const Vector3& v_old_to_new, const Vector3& oldAngleVel) const
	{
		return Acceleration(LinearAcc+CrossProd(AngleAcc,v_old_to_new)+CrossProd(oldAngleVel,CrossProd(oldAngleVel,v_old_to_new)), AngleAcc);
	}

	Acceleration Acceleration::RefBaseFrame ( const Frame3& old_base_in_new_base) const
	{
		return Acceleration(old_base_in_new_base.Rotation * LinearAcc, old_base_in_new_base.Rotation * AngleAcc);
	}

	bool Equal(const Acceleration& a,const Acceleration& b,const double eps) 
	{
		return (Equal(a.AngleAcc,b.AngleAcc,eps) && Equal(a.LinearAcc,b.LinearAcc,eps)  );
	}
}}